#ifndef __MOTOR_CTRL_H__
#define __MOTOR_CTRL_H__

#include "main.h"
#include "vmc.h"

#define LEFT  1
#define RIGHT 2

// motor1_angle = motor1_base_angle + 203.24f;
// motor2_angle = motor2_base_angle - 23.24f;
// motor4_angle = 360.0f - (motor4_base_angle + 156.76f);
// motor5_angle = 360.0f - (motor5_base_angle + 23.24f);

typedef struct
{
  /* data */
  float pos;
  float speed;
  float last_pos;
  float delta_pos;
  float set_torque;
  float angle;
  float delta_angle;
  float real_angle;
  float clear_flag;
} motor_data;

extern motor_data left_f;
extern motor_data left_b;
extern motor_data left_w;
extern motor_data right_f;
extern motor_data right_b;
extern motor_data right_w;

void Enable_All_Motor(void);
void Motor_Control(void);
void Motor_Rad_Cal(motor_data *data, float offset);
void Set_current(motor_data *motor1, motor_data *motor2, VMC *vmc, uint8_t side);
#endif
